COMMENT ⊗   VALID 00004 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	BEGIN "SPRING"
C00006 00003	PROCEDURE FETCHTV(REFERENCE INTEGER PIX)
C00009 00004	OUTSTR("FIELD OF VIEW (DEGREES):") FIELD←3.1415926*CVD(INCHWL)/180
C00018 ENDMK
C⊗;
BEGIN "SPRING"
REQUIRE "WINNIC.SAI[VIS,HPM]" SOURCE_FILE;

INTEGER ARRAY CHAN[0:5],FEAT[1:3,1:4,1:200],PICSZ[1:4];
INTEGER DX1,DY1,DX2,DY2,SCANS,SCANX,SCANY,ERRX,ERRY,H,BST,T,BSTBST;
INTEGER WINDOW,I,J,MAC,NP,PC,NF,CORWIN;
REAL SUM,TX,TY,T0,X,Y,XU,YU,TXP,TYP,FIELD,SCRN;
STRING ARRAY FILN[1:4],TAIL[1:4];
REAL ARRAY POS[1:2,1:4];
STRING DISP;
REAL X1H,Y1H,Z1H,X2H,Y2H,Z2H,D,TH0,X1,X2,R,XC,ZC;
REAL TH1,TH2,D1,D2,Y1,Z1,Y2,Z2,TT;

REAL PROCEDURE TAN(REAL X); RETURN(SIN(X)/COS(X));

PROCEDURE FLUSH(INTEGER F,PIC); COMMENT FORGET A FEATURE;
 BEGIN REAL X,Y,XU,YU;
 X←(FEAT[1,PIC,F]-1)/PICWID+POS[1,PIC]; XU←X+TXP;
 Y←(FEAT[2,PIC,F]-1)/PICHIG+POS[2,PIC]; YU←Y+TYP;
 DRKEN;
 LINE(X,Y,XU,Y);
 LINE(XU,Y,XU,YU);
 LINE(XU,YU,X,YU);
 LINE(X,YU,X,Y);
 TXTPOS(X-.02,Y+.04,.02,-.04); TEXT((F MOD 26)+'100);
 LITEN;
 END;

PROCEDURE SHOW(INTEGER F,PIC); COMMENT DISPLAY A FEATURE;
 BEGIN REAL X,Y,XU,YU;
 X←(FEAT[1,PIC,F]-1)/PICWID+POS[1,PIC]; XU←X+TXP;
 Y←(FEAT[2,PIC,F]-1)/PICHIG+POS[2,PIC]; YU←Y+TYP;
 LITEN;
 LINE(X,Y,XU,Y);
 LINE(XU,Y,XU,YU);
 LINE(XU,YU,X,YU);
 LINE(X,YU,X,Y);
 TXTPOS(X-.02,Y+.04,.02,-.04); TEXT((F MOD 26)+'100);
 END;

STRING PROCEDURE HORSOL;
 BEGIN
 D←1; COMMENT USE CART TRAVEL AS UNIT OF DISTANCE;
 Z1H←FIELD*((FEAT[1,1,H]+WINDOW/2)-(PICWID+1)/2)/(PICWID-1);
 Z2H←FIELD*((FEAT[1,2,H]+WINDOW/2)-(PICWID+1)/2)/(PICWID-1);
 TH0←Z2H-Z1H;
 R←D/TH0;
 XC←R*SIN(TH0);
 ZC←R*(1-COS(TH0));
 SETFORMAT(0,1);
 RETURN(CVF(TH0*180/3.14159)&"'  ("&CVF(XC)&","&CVF(ZC)&")");
 END;

STRING PROCEDURE CAMSOL(INTEGER I);
 BEGIN
 TH1←FIELD*((FEAT[1,1,I]+WINDOW/2)-(PICWID+1)/2)/(PICWID-1);
 TH2←FIELD*((FEAT[1,2,I]+WINDOW/2)-(PICWID+1)/2)/(PICWID-1);
 TH2←TH2-TH0;
 TT←COS(TH2)*SIN(TH1)-SIN(TH2)*COS(TH1);
 D2←(ZC*COS(TH1)-XC*SIN(TH1))/TT;
 IF ABS(TT)<.001 ∨ ABS(D2)>200 THEN RETURN(" INFINITY'");
 D1←(ZC+D2*SIN(TH2))/SIN(TH1);
 IF ABS(TH1)<1/20 THEN RETURN(" TOO NEAR CENTER") ELSE
 IF D1>0∧D2>0∧D1>D2  THEN RETURN(CVF(D1)&"/"&CVF(D2)) ELSE
  IF D1<-90∨D2<-90 THEN RETURN(" INFINITY""") ELSE RETURN("   ?");
 END;

PROCEDURE RECTIFY(REFERENCE INTEGER PIC1,PIC2);
 BEGIN
 REAL DEL,XTN,HI,A; INTEGER I,J,K;
 INTEGER ARRAY POS[1:PICWID];
 DEL←(PICWID+1)/2;
 XTN←FIELD/(PICWID-1);
 HI←TAN(FIELD/2);
 A←(PICWID-1)/(2*HI);
 FOR I←1 STEP 1 UNTIL PICWID DO
  BEGIN
  K←A*TAN((I-DEL)*XTN)+DEL+.5;
  IF K<1∨K>PICWID THEN OUTSTR("LOSE, K="&CVS(K)&'15&'12);
  FOR J←1 STEP 1 UNTIL PICHIG DO PUTEL(PIC2,J,I,PIXEL(PIC1,J,K));
  END;
 END;

PROCEDURE FETCHTV(REFERENCE INTEGER PIX);
 BEGIN
 INTEGER ARRAY TEMPIC[1:PICSIZ];
 GETTV(4,TEMPIC[1]);
 CLEAN(TEMPIC[1]); PASSHI(TEMPIC[1],PIX);
 END;

PROCEDURE INITDISPLAY;
IF DISP="Y" THEN
 BEGIN
 POS[1,1]←-1.1; POS[2,1]←-1.1; POS[1,2]←-1.1; POS[2,2]←.1; NP←2;
 CHAN[0]←'32; CHAN[1]←'37; CHAN[2]←'33; CHAN[3]←'34; CHAN[4]←'35; CHAN[5]←'36;
 FOR I←0 STEP 1 UNTIL 5 DO GDDCHN(CHAN[I]);
 SCRN←0;
 DDINIT; SCREEN(-1.13-SCRN,1.2+SCRN,1.0+SCRN,-1.2-SCRN);
 DRKEN; RECTAN(-2,2,2,-2);
 END;

PROCEDURE PICDISPLAY;
 BEGIN
  IF DISP ="Y" THEN
   BEGIN
   FOR I←1 STEP 1 UNTIL 5 DO DPYUP('36);

   FOR I←4 STEP -1 UNTIL (5-PICBIT) MAX 0 DO
    BEGIN
    FOR PC←1 STEP 1 UNTIL NP DO
     BEGIN
     DRKEN; RECTAN(POS[1,PC],POS[2,PC],POS[1,PC]+1,POS[2,PC]+1); LITEN;
     VIDEO(POS[1,PC],POS[2,PC],POS[1,PC]+1,POS[2,PC]+1,PIC[PC,1],2↑(PICBIT+I-5));
     LINE(POS[1,PC],POS[2,PC],POS[1,PC]+1,POS[2,PC]);
     LINE(POS[1,PC]+1,POS[2,PC],POS[1,PC]+1,POS[2,PC]+1);
     LINE(POS[1,PC]+1,POS[2,PC]+1,POS[1,PC],POS[2,PC]+1);
     LINE(POS[1,PC],POS[2,PC]+1,POS[1,PC]+1,POS[2,PC]);
     END;
    FOR J←1,2,3,4 DO DPYUP(CHAN[I]);
    END;
   END;

  DRKEN; RECTAN(-2,2,2,-2); LITEN;
  FOR J←0 STEP 1 UNTIL HORWIN DO
   BEGIN
   X←J*TX+POS[1,1];
   LINE(X,POS[2,1],X,POS[2,1]-.02);
   LINE(X,POS[2,1]+1,X,POS[2,1]+1.02);
   END;
  FOR I←0 STEP 1 UNTIL VERWIN DO
   BEGIN
   Y←I*TY+POS[2,1];
   LINE(POS[1,1],Y,POS[1,1]-.02,Y);
   LINE(POS[1,1]+1,Y,POS[1,1]+1.02,Y);
   END;

  DPYUP(CHAN[5]);
 END;
OUTSTR("FIELD OF VIEW (DEGREES):"); FIELD←3.1415926*CVD(INCHWL)/180;
OUTSTR("FEATURE WINDOW SIZE:"); WINDOW←CVD(INCHWL);
OUTSTR("CORRELATION WINDOW SIZE:"); CORWIN←CVD(INCHWL);
INIWIN(2*CORWIN);
OUTSTR("DISPLAY?"); DISP←INCHWL LAND '137;

INITDISPLAY;
MAKPIX(200,200,4);
INIWIN(WINDOW);

 BEGIN
 INTEGER ARRAY PIC[1:NP,1:PICSIZ]; INTEGER WHIPIX;
 
 TX←WINDOW/PICWID; TXP←(WINDOW-1)/PICWID;
 TY←WINDOW/PICHIG; TYP←(WINDOW-1)/PICHIG;
 WHIPIX←1;
 FETCHTV(PIC[WHIPIX,1]); FORWARD; FETCHTV(PIC[3-WHIPIX,1]);

  BEGIN
  INTEGER ARRAY WINS[-1:VERWIN,1:HORWIN];

  SETFORMAT(0,1);
  SUM←DOWIN(PIC[1,1],WINS[1,1]); 

  NF←0;
  FOR I←2 STEP 1 UNTIL VERWIN-3 DO
  FOR J←3 STEP 1 UNTIL HORWIN-2 DO
   BEGIN  INTEGER II,JJ,WMAX;
   X←(J-1)*TX+POS[1,1];
   Y←(I-1)*TY+POS[2,1];
   IF (WINS[I,J] LAND 1)=0 ∧ WINS[I,J]≥SUM THEN
    BEGIN
    FEAT[1,1,NF←NF+1]←(J-1)*WINDOW+1;
    FEAT[2,1,NF]←(I-1)*WINDOW+1;
    FEAT[3,1,NF]←WINS[I,J];
    SHOW(NF,1);
    END;
   END;
 COMMENT FIND HORIZON;
 WINDOW←CORWIN;
 INIWIN(WINDOW);
 TX←WINDOW/PICWID;
 TY←WINDOW/PICHIG;
 BSTBST←'377777000000;
  DO
   BEGIN
   H←1; BST←0;
   FOR I←1 STEP 1 UNTIL NF%2 DO
   IF FEAT[3,1,I]>BST ∧ FEAT[3,1,I]<BSTBST ∧
      FEAT[1,1,I]≥PICWID/3 ∧ FEAT[1,1,I]≤2*PICWID/3 THEN
	 BEGIN H←I; BST←FEAT[3,1,I]; END;
   BSTBST←BST;
   SCANY←10; SCANX←60;
   DX1←1 MAX (FEAT[1,1,H]-SCANX);
   DY1←1 MAX (FEAT[2,1,H]-SCANY);
   DX2←PICWID MIN (FEAT[1,1,H]+SCANX+WINDOW-1);
   DY2←PICHIG MIN (FEAT[2,1,H]+SCANY+WINDOW-1);
   T0←CALL(0,"RUNTIM");
   T←BSTCOR(PIC[1,1],FEAT[1,1,H],FEAT[2,1,H],FEAT[1,1,H]+WINDOW-1,
		 FEAT[2,1,H]+WINDOW-1,PIC[2,1],DX1,DY1,DX2,DY2);
   FEAT[1,2,H]←DX1; FEAT[2,2,H]←DY1; FEAT[3,2,H]←T;
   END UNTIL T≤8∨BST=0;
  OUTSTR("HORIZON SEARCH TIME -"&CVF((CALL(0,"RUNTIM")-T0)/1000)&" SECS"&'15&'12);
  IF BST=0 THEN
   BEGIN
   TXTPOS(.05,H*.08-1,.05,-.07);
   TEXT("HORIZON NOT FOUND");
   FOR J←1,2,3 DO DPYUP(CHAN[5]);
   OUTSTR("COULDN'T LOCATE HORIZON"&'15&'12);
   END
  ELSE
   BEGIN
   SHOW(H,2);
   FOR J←1,2 DO
    BEGIN
    X←(FEAT[1,J,H]-FEAT[1,1,H])/PICWID+POS[1,J]+.5;
    Y←(FEAT[2,J,H]-FEAT[2,1,H])/PICHIG+POS[2,J]+.5;
    YU←POS[2,J]; XU←POS[1,J];
    LINE(X,YU,X,YU+1); LINE(XU,Y,XU+1,Y);
    END;
   TXTPOS(.05,H*.08-1,.05,-.07);
   TEXT(((H MOD 26)+'100)&"-");
   TXTPOS(.175,H*.08-1.01,.03,-.06);
   TEXT("(HORIZ) "&HORSOL);
   FOR J←1,2,3 DO DPYUP(CHAN[5]);
   ERRX←DX1-FEAT[1,1,H]; ERRY←DY1-FEAT[2,1,H];

   COMMENT NOW BEGIN TRACKING;
   T0←0;
   DO
    BEGIN




 END;
END;